#include <geometry_msgs/Quaternion.h>
#include <ros/ros.h>
#include <serial/serial.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/Temperature.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
#include <string>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/tf.h>
#include <iostream>

//重置函数
bool zero_orientation_set = false;
bool set_zero_orientation(std_srvs::Empty::Request&,std_srvs::Empty::Response&)
{
  ROS_INFO("Zero Orientation Set.");
  zero_orientation_set = false;
  return true;
}

//imu结构体数据
typedef struct _imu_data_
{
  //线加速度
  int16_t ax,ay,az;
  double axf,ayf,azf;
  //角加速度
  int16_t gx,gy,gz;
  double gxf,gyf,gzf;
  //磁场
  int16_t mx,my,mz;
  double mxf,myf,mzf;
  //欧拉角及四元数
  int16_t roll,pitch,yaw; 
  double rollf,pitchf,yawf; 
  //int16_t qw,qx,qy,qz;
  double qwf,qxf,qyf,qzf;
  //温度
  int16_t temperature;
  double temperature_in_C;
}Imu_Data;

//数据转换函数 输入字符串 高位位置 低位位置 转换成整数数据
int16_t data_trans(const std::string input,int high,int low)
{
  return (((0xff&(char)input[high]) << 8) | 0xff&(char)input[low]);
}

//主函数
int main(int argc, char** argv)
{
  serial::Serial ser; //实例化串口对象
  std::string port;  //端口设备名 /dev/imu
  std::string tf_parent_frame_id; //tf父坐标名称
  std::string tf_frame_id; //tf子坐标名称
  std::string frame_id; //陀螺仪tf坐标名
  geometry_msgs::Quaternion q; //陀螺仪实时发布的四元数消息数据
  tf::Quaternion orientation; //陀螺仪实时发布的四元数消息数据
  tf::Quaternion zero_orientation; //初始的四元数数据
  tf::Quaternion differential_rotation; //相对的位姿,对外发布的四元数数据
  sensor_msgs::Imu imu; //imu消息数据
  sensor_msgs::MagneticField mag; //罗盘数据
  sensor_msgs::Temperature temperature_msg; //温度传感器数据变量

  double time_offset_in_seconds;
  bool broadcast_tf;  //是否发布TF变换
  double linear_acceleration_stddev; //线速度协方差矩阵数值
  double angular_velocity_stddev; //角速度协方差矩阵数值
  double orientation_stddev; //姿态协方差矩阵数值
  double magnetic_stddev; //罗盘协方差矩阵数值

  uint8_t last_received_message_number;
  bool received_message = false;
  int start_pos; //消息开始位置
  const int data_length=32; //数据长度
  Imu_Data  imu_data; //创建串口接收数据结构体
  memset(&imu_data, 0, sizeof(imu_data)); //清空结构体数据

  std::string input; //串口输入完整字符串变量
  std::string read; //串口读取的单个数据
  ros::init(argc, argv, "usb_imu_ros");
  ros::NodeHandle private_node_handle("~");
  private_node_handle.param<std::string>("port", port, "/dev/ttyUSB0");
  private_node_handle.param<std::string>("tf_parent_frame_id", tf_parent_frame_id, "imu_link");
  private_node_handle.param<std::string>("tf_frame_id", tf_frame_id, "imu");
  private_node_handle.param<std::string>("frame_id", frame_id, "imu_link");
  private_node_handle.param<double>("time_offset_in_seconds", time_offset_in_seconds, 0.0);
  private_node_handle.param<bool>("broadcast_tf", broadcast_tf, false);
  private_node_handle.param<double>("linear_acceleration_stddev", linear_acceleration_stddev, 0.000001);
  private_node_handle.param<double>("angular_velocity_stddev", angular_velocity_stddev, 0.0000001);
  private_node_handle.param<double>("orientation_stddev", orientation_stddev, 0.0000001);
  private_node_handle.param<double>("magnetic_stddev", magnetic_stddev, 0.000001);

  ros::NodeHandle nh("imu");
  ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("data", 50);
  ros::Publisher imu_temperature_pub = nh.advertise<sensor_msgs::Temperature>("temperature", 50);
  ros::Publisher mag_pub = nh.advertise<sensor_msgs::MagneticField>("magneticfield", 50);
  ros::ServiceServer service = nh.advertiseService("set_zero_orientation", set_zero_orientation);
  ros::Rate r(200); // 200 hz

  imu.linear_acceleration_covariance[0] = linear_acceleration_stddev;
  imu.linear_acceleration_covariance[4] = linear_acceleration_stddev;
  imu.linear_acceleration_covariance[8] = linear_acceleration_stddev;

  imu.angular_velocity_covariance[0] = angular_velocity_stddev;
  imu.angular_velocity_covariance[4] = angular_velocity_stddev;
  imu.angular_velocity_covariance[8] = angular_velocity_stddev;

  imu.orientation_covariance[0] = orientation_stddev;
  imu.orientation_covariance[4] = orientation_stddev;
  imu.orientation_covariance[8] = orientation_stddev;
  
  mag.magnetic_field_covariance[0]=magnetic_stddev;
  mag.magnetic_field_covariance[4]=magnetic_stddev;
  mag.magnetic_field_covariance[8]=magnetic_stddev;

  temperature_msg.variance = 0;

  static tf::TransformBroadcaster tf_br;
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(0,0,0));

  while(ros::ok())
  {
    try
    {
      if (ser.isOpen())
      {
        // 从串口读数据
        if(ser.available())
        {
          read = ser.read(ser.available());
          input += read;
          while (input.length() >= data_length) //当拿到一个完整长度的数据 
          {
            start_pos= input.find(0xA4); //寻找帧头0X04位置;
            if (start_pos!= std::string::npos)
            {
              if ((input.length() >= start_pos+ data_length)) 
              {
                // 线加速度数据
                imu_data.ax=data_trans(input,(start_pos+ 5),(start_pos+ 4));
                imu_data.ay=data_trans(input,(start_pos+ 7),(start_pos+ 6));
                imu_data.az=data_trans(input,(start_pos+ 9),(start_pos+ 8));
                //ROS_INFO("AX: %d  AY: %d AZ: %d ",imu_data.ax,imu_data.ay,imu_data.az);
                imu_data.axf=imu_data.ax/2048.0;
                imu_data.ayf=imu_data.ay/2048.0;
                imu_data.azf=imu_data.az/2048.0;
                //ROS_INFO("AX: %f  AY: %f AZ: %f ",imu_data.axf,imu_data.ayf,imu_data.azf);

                // 角加速度数据
                imu_data.gx=data_trans(input,(start_pos+ 11),(start_pos+ 10));
                imu_data.gy=data_trans(input,(start_pos+ 13),(start_pos+ 12));
                imu_data.gz=data_trans(input,(start_pos+ 15),(start_pos+ 14));
                imu_data.gxf=imu_data.gx/16.4;
                imu_data.gyf=imu_data.gy/16.4;
                imu_data.gzf=imu_data.gz/16.4;
                //ROS_INFO("GX: %f  GY: %f GZ: %f ",imu_data.gxf,imu_data.gyf,imu_data.gzf);
                
                //欧拉角
                imu_data.roll=data_trans(input,(start_pos+ 17),(start_pos+ 16));
                imu_data.pitch=data_trans(input,(start_pos+ 19),(start_pos+ 18));
                imu_data.yaw=data_trans(input,(start_pos+ 21),(start_pos+ 20));
                imu_data.rollf=imu_data.roll/100.0/360*6.2831852;
                imu_data.pitchf=imu_data.pitch/100.0/360*6.2831852;
                imu_data.yawf=imu_data.yaw/100.0/360*6.2831852;               
                //ROS_INFO( "001-ROLL-X: %f    PITCH-Y: %f    YAW-Z: %f ",imu_data.rollf,imu_data.pitchf,imu_data.yawf);
                
                q=tf::createQuaternionMsgFromRollPitchYaw(imu_data.rollf,imu_data.pitchf,imu_data.yawf);
                tf::Quaternion orientation(q.x,q.y,q.z,q.w);
                //重置函数
                if (!zero_orientation_set)
                {
                  zero_orientation = orientation;
                  zero_orientation_set = true;
                }
                differential_rotation = zero_orientation.inverse() * orientation; //相对重置后的姿态
                
                // 获取温度
                imu_data.temperature=data_trans(input,(start_pos+ 24),(start_pos+ 23));
                imu_data.temperature_in_C = imu_data.temperature/100.0;

                // 获取罗盘数据
                imu_data.mx = data_trans(input,(start_pos+ 26),(start_pos+ 25));
                imu_data.my = data_trans(input,(start_pos+ 28),(start_pos+ 27));
                imu_data.mz = data_trans(input,(start_pos+ 30),(start_pos+ 29));
                imu_data.mxf=imu_data.mx/37500000;
                imu_data.myf=imu_data.my/37500000;
                imu_data.mzf=imu_data.mz/37500000;
                //ROS_INFO("MX: %f  MY: %f MZ: %f ",imu_data.mxf,imu_data.myf,imu_data.mzf);

                //获取四元数，需要修改模块串口发布消息长度 默认不发四元数
                // imu_data.qw=data_trans(input,(start_pos+ 32),(start_pos+ 31));
                // imu_data.qx=data_trans(input,(start_pos+ 34),(start_pos+ 33));
                // imu_data.qy=data_trans(input,(start_pos+ 36),(start_pos+ 35));
                // imu_data.qz=data_trans(input,(start_pos+ 38),(start_pos+ 37));
                // //ROS_INFO("QW: %d  QX: %d  QY: %d QZ: %d ",imu_data.qw,imu_data.qx,imu_data.qy,imu_data.qz);
                // imu_data.qwf=imu_data.qw/16384.0;
                // imu_data.qxf=imu_data.qx/16384.0;
                // imu_data.qyf=imu_data.qy/16384.0;
                // imu_data.qzf=imu_data.qz/16384.0;
                // //ROS_INFO("QW: %f  QX: %f  QY: %f QZ: %f ",imu_data.qwf,imu_data.qxf,imu_data.qyf,imu_data.qzf);

                //检查是否有漏掉的消息
                uint8_t received_message_number = input[start_pos+ 3];
                if (received_message) 
                {
                  uint8_t message_distance = received_message_number - last_received_message_number;
                  if ( message_distance > 1 )
                  {
                    ROS_WARN_STREAM("Missed " << message_distance - 1 << " IMU data packets from stm32.");
                  }
                }
                else
                {
                  received_message = true;
                }
                last_received_message_number = received_message_number;

                // 计算测量时间 time_offset_in_seconds可以调整发布时间 默认为0
                ros::Time measurement_time = ros::Time::now() + ros::Duration(time_offset_in_seconds);

                // 发布imu数据ros消息
                imu.header.stamp = measurement_time;
                imu.header.frame_id = frame_id;
                imu.linear_acceleration.x = imu_data.axf;
                imu.linear_acceleration.y = imu_data.ayf;
                imu.linear_acceleration.z = imu_data.azf;
                imu.angular_velocity.x = imu_data.gxf;
                imu.angular_velocity.y = imu_data.gyf;
                imu.angular_velocity.z = imu_data.gzf;
                imu.orientation.x=differential_rotation.x();
                imu.orientation.y=differential_rotation.y();
                imu.orientation.z=differential_rotation.z();
                imu.orientation.w=differential_rotation.w();
                //quaternionTFToMsg(differential_rotation.inverse() , imu.orientation);
                imu_pub.publish(imu);
                //ROS_INFO("2-QX: %f  QY: %f  QZ: %f QW: %f ",imu.orientation.x,imu.orientation.y,imu.orientation.z,imu.orientation.w);
                
                //发布罗盘数据
                mag.header.stamp = measurement_time;
                mag.header.frame_id = frame_id;
                mag.magnetic_field.x=imu_data.mxf;
                mag.magnetic_field.y=imu_data.myf;
                mag.magnetic_field.z=imu_data.mzf;
                mag_pub.publish(mag);

                //发布温度数据
                temperature_msg.header.stamp = measurement_time;
                temperature_msg.header.frame_id = frame_id;
                temperature_msg.temperature = imu_data.temperature_in_C;
                imu_temperature_pub.publish(temperature_msg);

                // 发布TF变换
                if (broadcast_tf)
                {
                  transform.setRotation(differential_rotation);
                  tf_br.sendTransform(tf::StampedTransform(transform, measurement_time, tf_parent_frame_id, tf_frame_id));
                }
                //清空串口读取的数据变量input
                input.erase(0, start_pos+ data_length); 
              }
              else
              {
                if (input.length() >= start_pos+ data_length)
                {
                  input.erase(0, start_pos+ 1); 
                }
                else
                {
                  input.erase(0, start_pos);
                }
              }
            }
            else
            {
              input.clear();
            }
          }
        }
      }
      else
      {
        // 尝试打开串口
        try
        {
          ser.setPort(port);
          ser.setBaudrate(115200);
          serial::Timeout to = serial::Timeout::simpleTimeout(1000);
          ser.setTimeout(to);
          ser.open();
        }
        catch (serial::IOException& e) //异常处理
        {
          ROS_ERROR_STREAM("Unable to open serial port " << ser.getPort() << ". Trying again in 5 seconds.");
          ros::Duration(5).sleep();
        }

        if(ser.isOpen())
        {
          ROS_DEBUG_STREAM("Serial port " << ser.getPort() << " initialized and opened.");
        }
      }
    }
    catch (serial::IOException& e)
    {
      ROS_ERROR_STREAM("Error reading from the serial port " << ser.getPort() << ". Closing connection.");
      ser.close();
    }
    ros::spinOnce();
    r.sleep();
  }
}
